#include <iostream>
#include <fstream>

using namespace std;

void convertYUVToRGB(unsigned char* yuv420p, unsigned char* rgb, int width, int height) 
{
    int y_size = width * height;
    int u_size = y_size / 4;
    int v_size = y_size / 4;
    int y_plane_offset = 0;
    int u_plane_offset = y_size;
    int v_plane_offset = y_size + u_size;
    int rgb_offset = 0;

    for (int i = 0; i < height; i++) {
        for (int j = 0; j < width; j++) {
            int y = yuv420p[y_plane_offset + i * width + j];
            int u = yuv420p[u_plane_offset + (i / 2) * width / 2 + j / 2] - 128;
            int v = yuv420p[v_plane_offset + (i / 2) * width / 2 + j / 2] - 128;
            int r = y + (1402 * v) / 100;
            int g = y - (34 * u) / 100 - (71 * v) / 100;
            int b = y + (1770 * u) / 100;
            if (r < 0) r = 0; else if (r > 255) r = 255;
            if (g < 0) g = 0; else if (g > 255) g = 255;
            if (b < 0) b = 0; else if (b > 255) b = 255;
            rgb[rgb_offset + 0] = r;
            rgb[rgb_offset + 1] = g;
            rgb[rgb_offset + 2] = b;
            rgb_offset += 3;
        }
    }
}

void saveBMP(unsigned char* rgb, int width, int height, const char* filename) {
    ofstream outfile(filename, ios::binary);
    outfile << "BM" << endl << width << endl << height << endl << 16 << endl << 0 << endl << 0 << endl << width * height * 3 + 54 << endl << 54 << endl << ";" << endl;
    for (int i = height - 1; i >= 0; i--) {
        for (int j = 0; j < width * 3; j++) {
            outfile << rgb[i * width * 3 + j];
            if (j % 3 == 2) outfile << " ";
        }
        outfile << endl;
    }
    outfile.close();
}

int main() {
    int width = 320; // 设置宽度和高度，这里以640x480为例
    int height = 240;



    FILE *yuvFile = NULL;
    yuvFile = fopen("/data/session.YUV", "rb");
    if(yuvFile == NULL)
    {
        printf("===Failed to open file==\n");
        return 1;
    }
    unsigned char* yuv420p = new unsigned char[width * height + (width * height / 4 * 2)]; // YUV420P格式的YUV数据，假设数据已经填充到数组中
    fread(yuv420p, 1, width * height + (width * height / 4 * 2),  yuvFile);
    fclose(yuvFile);
    
    
    unsigned char* rgb = new unsigned char[width * height * 3]; // RGB数据，用于存储转换后的数据
    convertYUVToRGB(yuv420p, rgb, width, height); // 将YUV数据转换为RGB数据
    saveBMP(rgb, width, height, "output.bmp"); // 将转换后的RGB数据保存为BMP文件，文件名为output.bmp
    delete[] yuv420p; // 释放YUV数据的内存空间
    delete[] rgb; // 释放RGB数据的内存空间
    return 0;
}